from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import os
import xacro
from moveit_configs_utils import MoveItConfigsBuilder
from moveit_configs_utils.launches import generate_demo_launch


"""
这个demo：加载了moveit2的功能配置，然后还有机械臂运行的规划node和获取位姿的node，只要传入pose就可以实现抓取
如果你只是想单纯的实验一下可行性的化，可以选择启动这个demo后，
在命令行输入ros2 run ee_goal_control test_pose_node。


"""
def generate_launch_description():
    # 配置MoveIt
    moveit_config = MoveItConfigsBuilder("ur", package_name="ur_config").to_moveit_configs()
    
    # 生成MoveIt演示launch描述
    demo_launch = generate_demo_launch(moveit_config)
    
    # 获取MoveIt生成的节点列表
    moveit_nodes = demo_launch.entities
    
    # 定义你自己的节点
    hardware_node = Node(
        name='hardware_moveit',
        package="hardware_move",
        executable="hardware_moveit",
    )

    ee_goal = Node(
        name='ee_goal_node',
        package='ee_goal_control',
        executable='ee_goal_node',
        output='screen',
        parameters=[]
    )   
    
    goal_pose = Node(
        name="goal_pose_node",
        package="ee_goal_control",
        executable="goal_pose_node",
        parameters=[]
    )
    
    # 合并所有节点
    nodes_to_run = [
        *moveit_nodes,  # 解包MoveIt节点
        hardware_node,
        ee_goal,
        # goal_pose,  # 根据需要取消注释
    ]
    
    return LaunchDescription(nodes_to_run)